#include "SCoop.h"
#include <EEPROM.h>
// 定义ADC引脚
const int xuanzhuan = A0;
const int fuyang = A1;
const int fangxiang = A2;
const int anquandai = A3;
const int kakou = A4;
const int zhongliang = A5;
const int huoer = A7;

const int RightRotation = 2;
const int LeftRotation = 3;
const int DownRotation = 4;
const int TopRotation = 5;
const int WhiteLED = 9;
const int RedLED = 10;
const int FAN = 11;


const int shortPressTime = 500;    // 短按时间（ms）
const int timeInterval_2s = 2000;  // 3秒时间间隔（ms）
const int timeInterval_3s = 3000;  // 3秒时间间隔（ms）

int rotateButtonState = 0;          // 旋转按键状态（0：未按下，1：左旋转按键按下，2：右旋转按键按下，3：双按键同时按下）
int pitchButtonState = 0;           // 俯仰按键状态（0：未按下，1：前倾按键按下，2：后仰按键按下，3：双按键同时按下）
unsigned long rotatePressTime = 0;  // 旋转按键按下的时间（ms）
unsigned long pitchPressTime = 0;   // 俯仰按键按下的时间（ms）
unsigned long startTime = 0;

unsigned long WhitelastFlashTime = 0;  // 上次闪烁的时间
unsigned long RedlastFlashTime = 0;    // 上次闪烁的时间
unsigned long blinkTimer = 0;          // 闪烁定时器

int fanSpeed = 0;  //风扇转速
int fanSpeedAddress = 3;
int WhiteBrightness = 0;  //白灯亮度值

//设置状态变量
//电机锁定状态
bool motorLock = false;
int motorLockAddress = 0;
//旋转锁定状态
bool rotateLock = false;
int rotateLockAddress = 1;
//座椅安装状态
bool installed = false;
//有人系安全带状态
bool dangerous = true;
//风扇状态
bool FANState = false;
int FANStateAddress = 2;
bool rotateStop = false;
bool isForward = false;

bool rotatemotorLock = false;
bool pitchmotorLock = false;
bool LeftisTaskOneRunning = false;  //线程1是否启用
bool Leftfirst = false;             //是否为第一次进入线程1

bool RightisTaskOneRunning = false;  //线程2是否启用
bool Rightfirst = false;             //是否为第一次进入线程2

// 存储数据变化的计数
int changeCount = 0;
void setup() {
  // 初始化串口通信
  Serial.begin(19200);
  mySCoop.start();
  //设置电机控制端口
  pinMode(LeftRotation, OUTPUT);
  digitalWrite(LeftRotation, LOW);
  pinMode(RightRotation, OUTPUT);
  digitalWrite(RightRotation, LOW);
  pinMode(TopRotation, OUTPUT);
  digitalWrite(TopRotation, LOW);
  pinMode(DownRotation, OUTPUT);
  digitalWrite(DownRotation, LOW);
  //获取电机锁定状态
  motorLock = EEPROM.read(motorLockAddress);
  //获取旋转锁定状态
  rotateLock = EEPROM.read(rotateLockAddress);
  //获取风扇开关状态
  FANState = EEPROM.read(FANStateAddress);
  //获取风扇转速
  fanSpeed = EEPROM.read(fanSpeedAddress);

  Serial.print("电机锁定状态:");
  Serial.print(motorLock);
  Serial.print("旋转锁定状态:");
  Serial.print(rotateLock);
  Serial.print("风扇开关状态:");
  Serial.print(FANState);
  Serial.print("风扇速度值:");
  Serial.print(fanSpeed);
  Serial.println("");
}

void loop() {

  yield();
  // 读取引脚的模拟值
  int xuanzhuanVal = analogRead(xuanzhuan);
  int fuyangVal = analogRead(fuyang);
  int anquandaiVal = analogRead(anquandai);
  int fangxiangVal = analogRead(fangxiang);
  int kakouoVal = analogRead(kakou);
  int zhongliangVal = analogRead(zhongliang);
  int huoerVal = analogRead(huoer);
  int RightRotationVal = digitalRead(2);
  int LeftRotationVal = digitalRead(3);

  //判断是否有人乘坐，，如果没人，则关闭风扇和灯光，防止耗电
  if (anquandaiVal < 300) {
    analogWrite(FAN, 0);
  } else {
    //开启风扇后灯光闪烁
    if (FANState == true) {
      fanSpeed = EEPROM.read(fanSpeedAddress);
      analogWrite(FAN, fanSpeed);
      if (millis() - blinkTimer >= 1500) {  // 闪烁间隔
        if (WhiteBrightness == 0) {
          WhiteBrightness = 255;
        } else {
          WhiteBrightness = 0;
        }
        analogWrite(WhiteLED, WhiteBrightness);
        blinkTimer = millis();  // 更新闪烁定时器
      }
    } else {
      analogWrite(FAN, 0);
      analogWrite(WhiteLED, 255);
    }
  }


  //判断ISOFIX接口是否固定
  //如果ISOFIX接口未正确固定，则红色LED灯急速闪烁
  //如果座椅有坐人，但未系安全带，则白色LED灯急速闪烁
  if (kakouoVal > 530 && kakouoVal < 580) {
    installed = true;
    analogWrite(RedLED, 0);
    //判座椅是否坐人，并且系安全带
    if (anquandaiVal < 300 && zhongliangVal > 500) {
      dangerous = true;
      // 如果数据小于 500，控制灯光闪烁
      unsigned long WhitecurrentTime = millis();  // 获取当前时间
      if (WhitecurrentTime - WhitelastFlashTime >= 200) {
        // 如果距离上次闪烁的时间已经超过 100ms
        analogWrite(WhiteLED, 255);             // 设置 PWM 输出为最大值（255）
        WhitelastFlashTime = WhitecurrentTime;  // 更新上次闪烁的时间
      } else {
        analogWrite(WhiteLED, 0);  // 设置 PWM 输出为最小值（0）
      }                            // 等待 100ms
    } else {
      dangerous = false;
    }
  } else {
    installed = false;
    analogWrite(WhiteLED, 0);
    // 如果数据小于 500，控制灯光闪烁
    unsigned long RedcurrentTime = millis();  // 获取当前时间
    if (RedcurrentTime - RedlastFlashTime >= 200) {
      // 如果距离上次闪烁的时间已经超过 100ms
      analogWrite(RedLED, 255);           // 设置 PWM 输出为最大值（255）
      RedlastFlashTime = RedcurrentTime;  // 更新上次闪烁的时间
    } else {
      analogWrite(RedLED, 0);  // 设置 PWM 输出为最小值（0）
    }                          // 等待 100ms
  }

  //四按键同时按下,电机锁定,红灯闪烁三次
  if (xuanzhuanVal > 540 && fuyangVal > 540) {
    delay(timeInterval_3s);
    if (xuanzhuanVal > 540 && fuyangVal > 540) {
      if (motorLock == true) {
        motorLock = false;
        EEPROM.write(motorLockAddress, motorLock);
        flashLedThreeTimes(WhiteLED, 3, 500);
        analogWrite(WhiteLED, 255);
        delay(timeInterval_2s);
      } else {
        motorLock = true;
        EEPROM.write(motorLockAddress, motorLock);
        flashLedThreeTimes(RedLED, 3, 500);
        delay(timeInterval_2s);
      }
    }
  } else {
    //获取旋转按键值，设置对应状态
    if (xuanzhuanVal > 380 && xuanzhuanVal < 440) {
      delay(100);
      if (xuanzhuanVal > 380 && xuanzhuanVal < 440) {
        if (rotateButtonState == 0) {
          rotateButtonState = 1;
          rotatePressTime = millis();
        }
      }
    } else if (xuanzhuanVal > 470 && xuanzhuanVal < 530) {
      delay(100);
      if (xuanzhuanVal > 470 && xuanzhuanVal < 530) {
        if (rotateButtonState == 0) {
          rotateButtonState = 2;
          rotatePressTime = millis();
        }
      }
    } else if (xuanzhuanVal > 530) {
      delay(100);
      if (xuanzhuanVal > 510) {
        if (rotateButtonState != 3) {
          rotateButtonState = 3;
          rotatePressTime = millis();
        }
      }
    } else {
      if (rotateButtonState == 1) {
        if (millis() - rotatePressTime < shortPressTime) {
          LeftisTaskOneRunning = true;
          Leftfirst = true;
          if (LeftRotationVal == HIGH || RightRotationVal == HIGH) {
            rotateStop = true;
          } else {
            rotateStop = false;
          }
        } else {
          digitalWrite(LeftRotation, LOW);
          digitalWrite(RightRotation, LOW);
        }
      } else if (rotateButtonState == 2) {
        if (millis() - rotatePressTime < shortPressTime) {
          RightisTaskOneRunning = true;
          Rightfirst = true;
          if (LeftRotationVal == HIGH || RightRotationVal == HIGH) {
            rotateStop = true;
          } else {
            rotateStop = false;
          }
        } else {
          digitalWrite(LeftRotation, LOW);
          digitalWrite(RightRotation, LOW);
        }
      } else if (rotateButtonState == 3) {
        //旋转按键同时按下超过两秒，锁定/解锁旋转功能
        if (millis() - rotatePressTime > timeInterval_2s) {
          if (rotateLock == true) {
            rotateLock = false;
            flashLedThreeTimes(WhiteLED, 2, 500);
            EEPROM.write(rotateLockAddress, rotateLock);
            analogWrite(WhiteLED, 255);
          } else {
            rotateLock = true;
            EEPROM.write(rotateLockAddress, rotateLock);
            flashLedThreeTimes(RedLED, 2, 500);
          }
        }
      }
      rotateButtonState = 0;
    }

    //获取俯仰按键值，设置对应状态
    if (fuyangVal > 380 && fuyangVal < 440) {
      delay(100);
      if (fuyangVal > 380 && fuyangVal < 440) {
        if (pitchButtonState == 0) {
          pitchButtonState = 1;
          pitchPressTime = millis();
        }
        if (pitchButtonState == 1) {
          if (millis() - pitchPressTime > shortPressTime) {
            if (installed == true && dangerous == false && motorLock == false) {
              digitalWrite(TopRotation, HIGH);
            }
            if (motorLock == true) {
              analogWrite(WhiteLED, 0);
              flashLedThreeTimes(RedLED, 2, 500);
              analogWrite(WhiteLED, 255);
            }
          }
        }
      }
    } else if (fuyangVal > 470 && fuyangVal < 530) {
      delay(100);
      if (fuyangVal > 470 && fuyangVal < 530) {
        if (pitchButtonState == 0) {
          pitchButtonState = 2;
          pitchPressTime = millis();
        }
        if (pitchButtonState == 2) {
          if (millis() - pitchPressTime > shortPressTime) {
            if (installed == true && dangerous == false && motorLock == false) {
              digitalWrite(DownRotation, HIGH);
            }
            if (motorLock == true) {
              analogWrite(WhiteLED, 0);
              flashLedThreeTimes(RedLED, 2, 500);
              analogWrite(WhiteLED, 255);
            }
          }
        }
      }
    } else if (fuyangVal > 530) {
      delay(100);
      if (fuyangVal > 530) {
        if (pitchButtonState != 3) {
          pitchButtonState = 3;
          pitchPressTime = millis();
        }
      }
    } else {
      if (pitchButtonState == 1) {
        if (millis() - pitchPressTime < shortPressTime) {
          // 按钮1短按，控制风力大小
          if (FANState == true) {
            fanSpeed += 27;
            if (fanSpeed > 255) {
              fanSpeed = 255;
            }
            EEPROM.write(fanSpeedAddress, fanSpeed);
            analogWrite(FAN, fanSpeed);
          }
        }
      } else if (pitchButtonState == 2) {
        if (millis() - pitchPressTime < shortPressTime) {
          // 按钮1短按，控制风力大小
          if (FANState == true) {
            fanSpeed -= 27;
            if (fanSpeed < 100) {
              fanSpeed = 100;
            }
            EEPROM.write(fanSpeedAddress, fanSpeed);
            analogWrite(FAN, fanSpeed);
          }
        }
      } else if (pitchButtonState == 3) {
        //俯仰按键同时按下超过两秒，开启/关闭风扇
        if (millis() - pitchPressTime > timeInterval_2s) {
          if (FANState == true) {
            FANState = false;
            EEPROM.write(FANStateAddress, FANState);
            analogWrite(FAN, 0);
          } else {
            FANState = true;
            EEPROM.write(FANStateAddress, FANState);
            fanSpeed = EEPROM.read(fanSpeedAddress);
            analogWrite(FAN, fanSpeed);
          }
        }
      }
      pitchButtonState = 0;
      digitalWrite(TopRotation, LOW);
      digitalWrite(DownRotation, LOW);
    }
  }
  // //定期打印数据变化的计数
  // Serial.print("旋转按键:");
  // Serial.print(xuanzhuanVal);
  // Serial.print(";俯仰按键:");
  // Serial.print(fuyangVal);
  // Serial.print(";安全带:");
  // Serial.print(anquandaiVal);
  // Serial.print(";方向:");
  // Serial.print(fangxiangVal);
  // Serial.print(";卡扣:");
  // Serial.print(kakouoVal);
  // Serial.print(";重量:");
  // Serial.print(zhongliangVal);
  // Serial.print(";霍尔:");
  // Serial.print(huoerVal);
  // Serial.print("安装信息:");
  // Serial.print(installed);
  // Serial.print(";是否安全:");
  // Serial.print(dangerous);
  // Serial.print(";电机锁定:");
  // Serial.print(motorLock);
  // Serial.print(";旋转锁定:");
  // Serial.println(rotateLock);
  // Serial.print(";风扇状态:");
  // Serial.println(FANState);
  // Serial.print(";俯仰按键值:");
  // Serial.print(pitchButtonState);
  // Serial.print(";旋转电机值：");
  // Serial.print(rotateButtonState);
  // Serial.print(";");
  // Serial.println(LeftRotationState);
  // Serial.print(LeftisTaskOneRunning);
  // Serial.print(";");
  // Serial.print(Leftfirst);
  // Serial.println("");
}

//使用 PWM 输出来控制指定IO接口灯的亮度和闪烁频率
void flashLedThreeTimes(int IO, int frequency, int duration) {
  for (int i = 0; i < frequency; i++) {
    // 点亮灯
    analogWrite(IO, 255);  // 设置 PWM 输出为最大值（255）
    delay(duration);       // 等待指定的时间
    // 熄灭灯
    analogWrite(IO, 0);  // 设置 PWM 输出为最小值（0）
    delay(duration);     // 等待指定的时间
  }
}


defineTask(TaskOne);     // 创建子线程1
void TaskOne::setup() {  // 线程1设定
}
void TaskOne::loop() {  //线程1循环
  int fangxiangVal = analogRead(fangxiang);
  //第一次进入循环
  if (LeftisTaskOneRunning == true && Leftfirst == true) {
    startTime = millis();
    Leftfirst = false;
    if (fangxiangVal > 100) {
      isForward = true;
    } else {
      isForward = false;
    }
  }
  if (LeftisTaskOneRunning == true && isForward == true) {
    if (millis() - startTime < 4000) {
      if (installed == true && dangerous == false && motorLock == false && rotateLock == false) {
        if (rotateStop == false) {
          digitalWrite(LeftRotation, HIGH);
        } else {
          LeftisTaskOneRunning = false;
          digitalWrite(LeftRotation, LOW);
          isForward = false;
        }
      }
    } else {  //超时自动停止
      LeftisTaskOneRunning = false;
      digitalWrite(LeftRotation, LOW);
      isForward = false;
    }
  } else if (LeftisTaskOneRunning == true && isForward == false) {
    if (installed == true && dangerous == false && motorLock == false && rotateLock == false) {
      if (rotateStop == false) {
        if (millis() - startTime < 5000) {
          digitalWrite(LeftRotation, HIGH);
        } else {
          digitalWrite(LeftRotation, LOW);
          isForward = false;
          LeftisTaskOneRunning = false;
        }
      } else {
        LeftisTaskOneRunning = false;
        digitalWrite(LeftRotation, LOW);
        isForward = false;
      }
      if (fangxiangVal > 100) {
        digitalWrite(LeftRotation, LOW);
        isForward = true;
        LeftisTaskOneRunning = false;
      }
    }
  }
}

defineTask(TaskTwo);     // 创建子线程2
void TaskTwo::setup() {  // 线程2设定
}
void TaskTwo::loop() {  //线程2循环
  int fangxiangVal = analogRead(fangxiang);
  //第一次进入循环
  if (RightisTaskOneRunning == true && Rightfirst == true) {
    startTime = millis();
    Rightfirst = false;
    if (fangxiangVal > 100) {
      isForward = true;
    } else {
      isForward = false;
    }
  }
  if (RightisTaskOneRunning == true && isForward == true) {
    if (millis() - startTime < 4000) {
      if (installed == true && dangerous == false && motorLock == false && rotateLock == false) {
        if (rotateStop == false) {
          digitalWrite(RightRotation, HIGH);
        } else {
          RightisTaskOneRunning = false;
          digitalWrite(RightRotation, LOW);
          isForward = false;
        }
      }
    } else {  //超时自动停止
      RightisTaskOneRunning = false;
      digitalWrite(RightRotation, LOW);
      isForward = false;
    }
  } else if (RightisTaskOneRunning == true && isForward == false) {
    if (installed == true && dangerous == false && motorLock == false && rotateLock == false) {
      if (rotateStop == false) {
        if (millis() - startTime < 5000) {
          digitalWrite(RightRotation, HIGH);
        } else {
          digitalWrite(RightRotation, LOW);
          RightisTaskOneRunning = false;
          isForward = false;
        }
      } else {
        RightisTaskOneRunning = false;
        digitalWrite(RightRotation, LOW);
        isForward = false;
      }
      if (fangxiangVal > 100) {
        digitalWrite(RightRotation, LOW);
        isForward = true;
        RightisTaskOneRunning = false;
      }
    }
  }
}